/**
 * @file port.c
 * @author your name (you@domain.com)
 * @brief 
 * @version 0.1
 * @date 2022-07-14
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#include "k2os.h"
#include "stm32f10x.h"


__asm u16_t port_cpu_xpsr_save(void)
{
    MRS     R0, PRIMASK
    CPSID   I
    BX      LR
}

__asm void port_cpu_xpsr_restore(u16_t new_value)
{
    MSR     PRIMASK, R0
    BX      LR
}

/**
 * @brief init the task's stack.
 * 
 * TODO: stack need align(refence FreeRTOS)
 * 
 * @param task 
 * @param arg 
 * @param stack 
 * @return os_stk_t* 
 */
os_stk_t *port_task_stack_init(void (*task)(void *arg), 
                            void *arg, 
                            os_stk_t *stack)
{
    stack--;
    *stack = 0x01000000u;                       /* xPSR */
    stack--;                        
    *stack = (u32_t)task;                       /* return address */
    stack--;
    *stack = (u32_t)os_task_return;             /* R14 (LR) */
    stack -= 5;                                 /* R12, R3, R2, R1, R0 */
    *stack = (u32_t)arg;

    stack -= 8;                                 /* R11, R10, R9, R8, R7, R6, R5, R4 */

    return stack;    
}

/**
 * @brief set the PendSV_Handler lowest priority
 * 
 */
void port_set_PendSV_Handler_lowest_priority(void)
{
    NVIC_SetPriority(PendSV_IRQn, 0xFF);
}


__asm void port_SVC_Handler(void)
{
    extern k2os_next_run_task_ptcb;
    extern k2os_running;

    PRESERVE8

    ldr r1, =k2os_next_run_task_ptcb
    ldr r1, [r1]
    ldr r0, [r1]
    ldmia r0!, {r4-r11}
    msr psp, r0 
    isb 

    ldr r0, =k2os_running
    movs r1, #1
    strb r1, [r0]
		
	  ldr     r0, =k2os_curr_running_priority
    ldr     r1, =k2os_next_run_priority 
    ldrb    r2, [r1] 
    strb    r2, [r0]                            /* set k2os_curr_running_priority = k2os_next_run_priority */

    ldr     r0, =k2os_curr_running_task_ptcb    
    ldr     r1, =k2os_next_run_task_ptcb
    ldr     r2, [r1]
    str     r2, [r0]                            /* set k2os_curr_running_task_ptcb = k2os_next_run_task_ptcb */

    mov r0, #0
    msr basepri, r0 

    orr r14, #0xd
    bx r14
}


__asm void port_PendSV_Handler(void)
{
    extern k2os_curr_running_task_ptcb;
    extern k2os_curr_running_priority;
    extern k2os_next_run_priority;

    PRESERVE8 

    cpsid   i                                   /* prevent interrupt during context switch */
    // save current task context
    mrs     r0, psp
    isb
    stmdb   r0!, {r4-r11}
    
    ldr     r1, =k2os_curr_running_task_ptcb        
    ldr     r1, [r1]                            
    str     r0, [r1]                            /* update the new psp to current task */


    ldr     r0, =k2os_curr_running_priority
    ldr     r1, =k2os_next_run_priority 
    ldrb    r2, [r1] 
    strb    r2, [r0]                            /* set k2os_curr_running_priority = k2os_next_run_priority */

    ldr     r0, =k2os_curr_running_task_ptcb    
    ldr     r1, =k2os_next_run_task_ptcb
    ldr     r2, [r1]
    str     r2, [r0]                            /* set k2os_curr_running_task_ptcb = k2os_next_run_task_ptcb */

    ldr     r0, [r2]                            /* get k2os_next_run_task_ptcb->pTopOfStack */
    ldmia   r0!, {r4-r11}
    msr     psp, r0
    isb
    cpsie   i  
    bx      r14 
    nop 
}

__asm void port_start_first_task(void)
{
    PRESERVE8

    /* Use the NVIC offset register to locate the stack. */
    ldr r0, =0xE000ED08
    ldr r0, [r0]
    ldr r0, [r0]

    /* set msp init */
    msr msp, r0

    /* enable interrupts */
    cpsie i 
    cpsie f 
    dsb 
    isb 

    /* call svc 0 to start the first task */
    svc 0
    nop 
    nop 
}


void port_Systick_init(void)
{
    u32_t reload;

    /* 1. set SysTick Handler lowest priority */
    NVIC_SetPriority(SysTick_IRQn, 0xFF);
    /* 2. set SysTick clock source */
    SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8);
    reload = SystemCoreClock / 8000000;
    /* 3. set SysTick reload cnt */
    reload *= (1000000 / OS_CFG_TICKS_WITHIN_ONE_SECOND);
    SysTick->CTRL |= SysTick_CTRL_TICKINT_Msk;
    SysTick->LOAD = reload;
    /* 4. start SysTick  */
    SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk; 
}


void port_trigger_PendSV_Handler(void)
{
    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
}


void os_ticks_increment(void);
void os_isr_exit(void);
void os_isr_nesting_inc(void);


void port_SysTick_Handler(void)
{
    os_cpu_psr_t cpu_psr = 0;

    if(0 == os_get_running_status()){
        return;
    }

    os_enter_critical();
    os_isr_nesting_inc();
    os_exit_critical();

    os_ticks_increment();
    os_isr_exit();
}






